#!/usr/bin/python
# -*- coding: utf-8 -*-


import usb  # requires pyusb available at https://sourceforge.net/projects/pyusb/files/

import sys
import time

import threading # Usada para hilos
import gobject  #  Se requiere un hilo para monitorear
                #  constantemente lo que se recibe



gobject.threads_init() # Inicializar hilos, al pedo porque por ahora no utiliza



class Pinguino():


    VENDOR = 0x04D8
    PRODUCT = 0xFEAA

    CONFIGURATION = 3
    INTERFACE = 0
    ENDPOINT_IN = 0x82
    ENDPOINT_OUT = 0x01

    device = None
    handle = None



    def __init__(self,):
        for bus in usb.busses():
            for dev in bus.devices:
                if dev.idVendor == self.VENDOR and dev.idProduct == self.PRODUCT:
                    self.device = dev
                    print "Device found. Vendor %x - Product %x" % (self.VENDOR, self.PRODUCT)
        return None

    def open(self):
        if not self.device:
            print "Error: Device %x:%x: Unable to find device!" % (self.VENDOR, self.PRODUCT)
            return None
        try:
            self.handle = self.device.open()
            self.handle.setConfiguration(self.CONFIGURATION)
            self.handle.claimInterface(self.INTERFACE)
        except usb.USBError, err:
            print err
            self.handle = None
        return self.handle

    def close(self):
        try:
            self.handle.releaseInterface()
        except Exception, err:
            print err
        self.handle, self.device = None, None

    def read(self, length, timeout = 100):
        stringRX = ''
        try:
            stringRX = self.handle.bulkRead(self.ENDPOINT_IN, length, timeout)
        except usb.USBError as err:
            pass
        return stringRX

    def write(self, buffer, timeout = 0):
        return self.handle.bulkWrite(self.ENDPOINT_OUT, buffer, timeout)


    def getInt(self, data):
        if len(data) == 2:
            return (data[1]*256) + data[0]
        else:
            return 0

    def getShort(self, data):
        if len(data) == 1:
            return data[0]
        else:
            return 0
        
    def getFloat(self, data):
        if len(data) == 2:
            return (data[1]*256) + data[0]
        else:
            return 0



if __name__ == '__main__':

    SET_COMMAND = 0xAA
    SET_TRAILER = 0x55
    
    GET_COMMAND = 0xF0
    GET_TRAILER = 0x0F
    
    pinguino = Pinguino()
    quit = False

    # rw - 1- Un valor mas alto a esto en el pin ACTUAL_STATE_PIN (10 bits) se considera nivel alto 
    high_pulse_value = 0

    # rw - 2- El valor mas alto detectado en el pin ACTUAL_STATE_PIN (10 bits) 
    highest_pulse_value = 0

    # rw - 3- Un valor mas bajo detectado en el pin ACTUAL_STATE_PIN (10 bits) 
    lowest_pulse_value = 0

    # rw - 4- estado actual del sistema, segun el medidor de pulsos 
    actual_state = 0

    # rw - 5- Posicion actual del servo actuador, en grados. Cuando el control esta inactivo actuator_position = 0, el maximo es ACTUATOR_MAP_BYTES grados 
    actuator_position = 0

    # rw - 6- setpoint seteada 
    setpoint = 0

    # rw - 7- Indica si el circuito ya esta configurado para usarse, no se iniciara un proceso de control de no ser asi 
    is_configured = False

    # rw - 8- Indica si el control activo en un momento dado 
    is_active = False

    # rw - 9- El valor de actual_state (valor de pulse_counter SPEED_MEASUREMENT_TIME tiempo) se multiplica por este coheficiente, es para ajustar el
    # lazo PID y que se comporte de la misma manera aún cuando el sensor de estado actual emite mas o menos pulsos para un mismo estado 
    speed_adjustment_coefficient = 0

    # r - 10- Error anterior 
    last_error = 0

    # r - 11- Valor de la proporcional del proceso PID 
    proporcional = 0

    # r - 12- Valor de la integrative del proceso PID 
    integrative = 0

    # r - 13- Valor de la derivative del proceso PID 
    derivative = 0

    # rw - 14- tiempo entre un periodo de control y otro, para los lazos PID que controlan sistema lentos el valor de 1 segundo es generalmente adecuado. 
    Dt = 0

    # rw - 15- Valor Kp (proporcional) del control pid
    Kp = 0

    # rw - 16- Valor Ki (integrative) del control pid
    Ki = 0

    # rw - 17- Valor Kd (derivative) del control pid
    Kd = 0

    # rw - 18- Debug activado o no, envia a la salida informacion de estado
    DEBUGGING = False
    
    # Version (tomada del SVN... cuando me acuerdo) - valor de variable r20
    VERSION = 0
    
   
    if pinguino.open() == None:
        exit(1)

    while(not quit):

        #pinguino.write(chr(SET_COMMAND) + chr(30) + chr(5) + chr(50) + chr(SET_TRAILER), 100)
    
        pinguino.write(chr(GET_COMMAND) + chr(1) + chr(0) + chr(0) + chr(GET_TRAILER), 100)
        high_pulse_value = pinguino.getInt(pinguino.read(2))

        pinguino.write(chr(GET_COMMAND) + chr(2) + chr(0) + chr(0) + chr(GET_TRAILER), 100)
        highest_pulse_value =  pinguino.getInt(pinguino.read(2))

        pinguino.write(chr(GET_COMMAND) + chr(3) + chr(0) + chr(0) + chr(GET_TRAILER), 100)
        lowest_pulse_value =  pinguino.getInt(pinguino.read(2))

        pinguino.write(chr(GET_COMMAND) + chr(4) + chr(0) + chr(0) + chr(GET_TRAILER), 100)
        actual_state =  pinguino.getInt(pinguino.read(2))

        pinguino.write(chr(GET_COMMAND) + chr(5) + chr(0) + chr(0) + chr(GET_TRAILER), 100)
        actuator_position = pinguino.getShort(pinguino.read(1))

        pinguino.write(chr(GET_COMMAND) + chr(6) + chr(0) + chr(0) + chr(GET_TRAILER), 100)
        setpoint =  pinguino.getInt(pinguino.read(2))

        pinguino.write(chr(GET_COMMAND) + chr(7) + chr(0) + chr(0) + chr(GET_TRAILER), 100)
        is_configured =  pinguino.getShort(pinguino.read(1)) == 1

        pinguino.write(chr(GET_COMMAND) + chr(8) + chr(0) + chr(0) + chr(GET_TRAILER), 100)
        is_active = pinguino.getShort(pinguino.read(1)) == 1

        pinguino.write(chr(GET_COMMAND) + chr(9) + chr(0) + chr(0) + chr(GET_TRAILER), 100)
        speed_adjustment_coefficient = pinguino.getFloat(pinguino.read(4))

        pinguino.write(chr(GET_COMMAND) + chr(10) + chr(0) + chr(0) + chr(GET_TRAILER), 100)
        last_error = pinguino.getFloat(pinguino.read(4))

        pinguino.write(chr(GET_COMMAND) + chr(11) + chr(0) + chr(0) + chr(GET_TRAILER), 100)
        proporcional = pinguino.getFloat(pinguino.read(4))

        pinguino.write(chr(GET_COMMAND) + chr(12) + chr(0) + chr(0) + chr(GET_TRAILER), 100)
        integrative = pinguino.getFloat(pinguino.read(4))

        pinguino.write(chr(GET_COMMAND) + chr(13) + chr(0) + chr(0) + chr(GET_TRAILER), 100)
        derivative = pinguino.getFloat(pinguino.read(4))

        pinguino.write(chr(GET_COMMAND) + chr(14) + chr(0) + chr(0) + chr(GET_TRAILER), 100)
        Dt = pinguino.getInt(pinguino.read(2))

        pinguino.write(chr(GET_COMMAND) + chr(15) + chr(0) + chr(0) + chr(GET_TRAILER), 100)
        Kp = pinguino.getFloat(pinguino.read(4))

        pinguino.write(chr(GET_COMMAND) + chr(16) + chr(0) + chr(0) + chr(GET_TRAILER), 100)
        Ki = pinguino.getFloat(pinguino.read(4))

        pinguino.write(chr(GET_COMMAND) + chr(17) + chr(0) + chr(0) + chr(GET_TRAILER), 100)
        Kd = pinguino.getFloat(pinguino.read(4))

        pinguino.write(chr(GET_COMMAND) + chr(18) + chr(0) + chr(0) + chr(GET_TRAILER), 100)
        DEBUGGING = pinguino.getShort(pinguino.read(1))

        pinguino.write(chr(GET_COMMAND) + chr(20) + chr(0) + chr(0) + chr(GET_TRAILER), 100)
        VERSION = pinguino.getInt(pinguino.read(2))

            
        print "high_pulse_value: " + str(high_pulse_value)
        print "highest_pulse_value: " + str(highest_pulse_value)
        print "lowest_pulse_value: " + str(lowest_pulse_value)
        print "actual_state: " + str(actual_state)
        print "actuator_position: " + str(actuator_position)
        print "setpoint: " + str(setpoint)
        print "is_configured: " + str(is_configured)
        print "is_active: " + str(is_active)
        print "speed_adjustment_coefficient: " + str(speed_adjustment_coefficient)
        print "last_error: " + str(last_error)
        print "proporcional: " + str(proporcional)
        print "integrative: " + str(integrative)
        print "derivative: " + str(derivative)
        print "Dt: " + str(Dt)
        print "Kp: " + str(Kp)
        print "Ki: " + str(Ki)
        print "Kd: " + str(Kd)
        print "DEBUGGING: " + str(DEBUGGING)
        print "VERSION: " + str(VERSION)

        #for i in range(255):
            #pinguino.write(chr(SET_COMMAND) + chr(30) + chr(i) + chr(19) + chr(SET_TRAILER), 1000)
            #pinguino.write(chr(GET_COMMAND) + chr(30) + chr(i) + chr(0) + chr(GET_TRAILER), 1000)
            #print "eeprom[%i]: %i" % (i, pinguino.read(10,100)[0])
            #print pinguino.read(10,100)
        
        pinguino.write(chr(GET_COMMAND) + chr(30) + chr(5) + chr(0) + chr(GET_TRAILER), 100)
        print pinguino.read(10)
        quit = True
        
        
    pinguino.close()
    print "Saliendo"
    exit(0)
